#ifndef SETUP_MODE_H
#define SETUP_MODE_H

#include "cncmode.h"
#include "kbdcmd.h"
#ifdef _DEBUG
#include <windows.h>
#endif

namespace cnc {

	namespace base {

		class SetupMode : public CNCMode
		{
			enum State {
				STATE_STAND_BY,
				STATE_X_AXIS_HOMING,
				STATE_Y_AXIS_HOMING,
				STATE_Z_AXIS_HOMING,
				STATE_CHANGE_MODE,
				STATE_EXIT
			} state_;
			cnc::base::OperationModeID nextMode_;
			State standBy()
			{
				cnc::comm::KbdCommand kbdCmd;
				if (keyboard_->getCommand(&kbdCmd)) {
					//uiService_->setCommand(kbdCmd);
					switch (kbdCmd.getID()) {
						case cnc::comm::AXIS::ID:
							switch (kbdCmd.getValue())
							{
								case cnc::comm::AXIS::X_POSITIVE_SELECT:
									return STATE_X_AXIS_HOMING;
								case cnc::comm::AXIS::Y_POSITIVE_SELECT:
									return STATE_Y_AXIS_HOMING;
								case cnc::comm::AXIS::Z_POSITIVE_SELECT:
									return STATE_Z_AXIS_HOMING;
							}
							break;
						case cnc::comm::MODE::ID:
							nextMode_ = opMode_[kbdCmd.getValue()];
							if (machOp_->isHome())
							{
								printCommand(kbdCmd);
								return STATE_CHANGE_MODE;
							}
							break;
					}
		#ifdef _DEBUG
					if (kbdCmd.getID() == 0xFFFF && kbdCmd.getValue() == 0xFFFF)
						return STATE_EXIT;
					printCommand(kbdCmd);
		#endif
				}
				return STATE_STAND_BY;
			}
			State homeAxis(int axis)
			{
				State state[] = {
					STATE_X_AXIS_HOMING,
					STATE_Y_AXIS_HOMING,
					STATE_Z_AXIS_HOMING
				};
		#ifdef _DEBUG
				char *axisName[] = { "X", "Y", "Z" };
				std::cout << axisName[axis] << " axis homed." << std::endl;
		#endif
				switch (machOp_->getAxisStatus(axis)) {
				case AS_HOME:
					return STATE_STAND_BY;
				case AS_IDLE:
				case AS_MOVING:
					return state[axis];
				case AS_ERROR:
					// display error
					return STATE_STAND_BY;
				}
				return STATE_STAND_BY;
			}
		public:
			SetupMode() : CNCMode(cnc::base::OPMODE_SETUP), state_(STATE_STAND_BY) {}
			virtual bool start()
			{
				uiService_->selectForm("zeroReturnMode");
				state_ = STATE_STAND_BY;
				return true;
			}
			virtual bool execute()
			{
				switch (state_) {
				case STATE_STAND_BY:
					state_ = standBy();
					break;
				case STATE_X_AXIS_HOMING:
					state_ = homeAxis(0);
					break;
				case STATE_Y_AXIS_HOMING:
					state_ = homeAxis(1);
					break;
				case STATE_Z_AXIS_HOMING:
					state_ = homeAxis(2);
					break;
				case STATE_CHANGE_MODE:
					if (selectMode(nextMode_))
					{
						state_ = STATE_STAND_BY;
						break;
					}
				case STATE_EXIT:
					return false;
				}
				return true;
			}
			virtual bool stop()
			{
				return true;
			}
		};

	} // namespace base

} // namespace cnc

#endif
